#include "Quaternion.h"
#include "Vectors.h"

#include <cmath>
#include <string>
#include <cstdlib>

using std::sin;
using std::cos;
using std::acos;
using namespace neiderra::math;

quat::quat(const vec3& v)
{
	x = v.x;
	y = v.y;
	z = v.z;
	w = 0.0f;
}

quat::quat(float X, float Y, float Z, float W)
{
    x = X;
	y = Y;
	z = Z;
	w = W;
}

quat::quat()
{
	x = y = z = 0.0f;
	w = 0.0f;
}

void quat::setRotateAboutAxis(vec3 Axis, float theta)
{
	vec3 axis = Axis;
	axis.normalize();

	theta*=0.5f;
	float sinTheta = sin(theta);

	x = axis.x * sinTheta;
	y = axis.y * sinTheta;
	z = axis.z * sinTheta;
	w = cos(theta);
}

void quat::setRotateAboutX(float theta)
{
	theta*=0.5f;
	x = sin(theta);
	y = 0.0f;
	z = 0.0f;
	w = cos(theta);
}

void quat::setRotateAboutY(float theta)
{
	theta*=0.5f;
	x = 0.0f;
	y = sin(theta);
	z = 0.0f;
	w = cos(theta);
}

void quat::setRotateAboutZ(float theta)
{
	theta*=0.5f;
	x = 0.0f;
	y = 0.0f;
	z = sin(theta);
	w = cos(theta);
}

void quat::identity()
{
	w = 1.0f;
	x = y = z = 0.0f;
}

quat quat::conjugate()
{
	quat q(-x, -y, -z, w);
	q.normalize();
	return q;
}

void quat::normalize()
{
	float mag = (float)sqrt(x*x+y*y+z*z+w*w);

	if(mag >0.0f)
	{
		float oneOverMag = 1.0f / mag;
		x*=oneOverMag;
		y*=oneOverMag;
		z*=oneOverMag;
		w*=oneOverMag;
	}
	else
	{
		identity();
	}
}

quat& quat::operator =(const quat& q)
{
	x = q.x;
	y = q.y;
	z = q.z;
	w = q.w;
	return *this;
}

quat quat::operator *(const quat& a)
{
	quat result;
	result.w = w*a.w - x*a.x - y*a.y - z*a.z;
	result.x = w*a.x + x*a.w + z*a.y - y*a.z;
	result.y = w*a.y + y*a.w + x*a.z - z*a.x;
	result.z = w*a.z + z*a.w + y*a.x - x*a.y;
	return result;
}
quat& quat::operator =(const vec3& v)
{
	x = v.x;
	y = v.y;
	z = v.z;
	w = 0.0f;
	return *this;
}

quat& quat::operator *=(const quat& q)
{
	*this = *this * q;
	return *this;
}

vec3 quat::getRotationAxis()
{
    float sinThetaOver2Sq = 1.0f - w*w;
	if(sinThetaOver2Sq <= 0.0f)
	{
		return vec3(1.0f, 0.0f, 0.0f);
	}
	float oneOverSinThetaOver2 = 1.0f/sqrt(sinThetaOver2Sq);
	return vec3(x*oneOverSinThetaOver2,
		y*oneOverSinThetaOver2,
		z*oneOverSinThetaOver2);
}

void neiderra::math::convertQuatToMatrix(quat& q, float* mat)
{
	q.normalize();
	const float 
		xx = q.x * q.x,
		xy = q.x * q.y,
		xz = q.x * q.z,
		xw = q.x * q.w,
		
		yy = q.y * q.y,
		yz = q.y * q.z,
		yw = q.y * q.w,
		
		zz = q.z * q.z,
		zw = q.z * q.w;

	mat[0] = 1-2*(yy+zz);
	mat[1] = 2*(xy-zw);
	mat[2] = 2*(xz+yw);
	
	mat[4] = 2*(xy+zw);
	mat[5] = 1-2*(xx+zz);
	mat[6] = 2*(yz-xw);

	mat[8] = 2*(xz-yw);
	mat[9] = 2*(yz+xw);
	mat[10] = 1-2*(xx+yy);

	mat[3] = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0;
	mat[15] = 1;
}

vec3 quat::toVec3()
{
	return vec3(x,y,z);
}

void clearBuf(char* b, int n)
{
	for(int i=0; i<n; i++)
		b[n] = 0;
}

void neiderra::math::convertStringToQuat(quat& q, const std::string& str)
{
	using namespace std;
    string s = str;
	math::trim(s);

	char buf[10];
	
	for(int n=0;n<10;n++) buf[n]=0;
	size_t oldi, i = s.find(',', 0);
	oldi = i+1;

	// x
	s.copy(buf, i, 0);
	q.x = atof(buf);
	for(int n=0;n<10;n++) buf[n]=0;
	i = s.find(',', oldi);

	// y
	s.copy(buf, i-oldi, oldi);
	oldi = i+1;
    q.y = atof(buf);
	for(int n=0;n<10;n++) buf[n]=0;
	i = s.find(',', oldi);

	// z
	s.copy(buf, i-oldi, oldi);
	oldi = i+1;
    q.z = atof(buf);
	for(int n=0;n<10;n++) buf[n]=0;

	s.copy(buf, 10, oldi);
    q.w = atof(buf);
}
